#include "PreProcess.h"
#include <string>

PreProcess::PreProcess() : nh_ ("~") {
  std::string scan_topic,processedTopic;
  nh_.param<std::string>("scan_topic", scan_topic, "/scan");
  nh_.param<std::string>("processed_topic", processedTopic, "/preprocessed");
  nh_.param<double>("cleanTh", cleanTh, 0.0);
  nh_.param<double>("noiseTh", noiseTh, 0.0);
  scan_sub_ = nh_.subscribe (scan_topic, 1, &PreProcess::newLaserScan, this);
  
  processed_pub = nh_.advertise<sensor_msgs::LaserScan>(processedTopic, 1);
  
  generator=std::default_random_engine((unsigned int)time(0));
  distribution=std::normal_distribution<double>(0.0,noiseTh);
}

PreProcess::PreProcess(const PreProcess& orig) {
}

PreProcess::~PreProcess() {
}

bool PreProcess::check(float r1, float r2, float th){
	//std::cout<<"r1: "<<r1<<"  r2: "<<r2<<"  cleanTh: "<<th<<"    : "<<(fabs(r1-r2)<th)<<std::endl;
  return (fabs(r1-r2)<th);
}

void PreProcess::newLaserScan(const sensor_msgs::LaserScan& scan_msg){
  scan_=scan_msg;
  if(noiseTh>0){
    for(int i=0;i<scan_.ranges.size();++i)
      scan_.ranges[i]+=distribution(generator);//noiseTh*((rand()%200)*0.01-1);
  }
  if(cleanTh>0){
    for(int i=0;i<scan_.ranges.size();++i){
      if(i==0){
        if(!check(scan_.ranges[i],scan_.ranges[i+1],cleanTh))
          scan_.ranges[i]==0;
      } else
      if(i==scan_.ranges.size()-1){
        if(!check(scan_.ranges[i],scan_.ranges[i-1],cleanTh))
          scan_.ranges[i]==0;
      } else
        if(!check(scan_.ranges[i],scan_.ranges[i-1],cleanTh) && !check(scan_.ranges[i],scan_.ranges[i+1],cleanTh))
          scan_.ranges[i]==0;
    }
  }
  processed_pub.publish(scan_);
}

int main(int argc, char** argv) {
  ros::init (argc, argv, "PreProcess");
  PreProcess proc;  
  ros::Rate loop_rate(10);
  while (ros::ok()) {
    ros::spinOnce();
    loop_rate.sleep();
  }
  return 0;
}
